﻿#include <opencv2/imgproc.hpp>
#include "calculate_tools.h"
#include "lane_const_parameter.h"

namespace rmd {

CalcuateTools* CalcuateTools::instance_ = NULL;

CalcuateTools::CalcuateTools() {}

CalcuateTools::~CalcuateTools() {}

CalcuateTools* CalcuateTools::GetInstance() {
  if (instance_ == nullptr) {
    instance_ = new CalcuateTools();
  }

  return instance_;
}

//已知2点，求第三点的x或y
cv::Point2f CalcuateTools::GetXorYToPt(cv::Point2f pt1, cv::Point2f pt2,
                                       double t, bool yFlag) {
  double b;
  if (yFlag) {  //已知y, 求x
    if (pt2.y - pt1.y == 0) {
      b = pt1.x;
    } else {
      b = ((pt2.x - pt1.x) / (pt2.y - pt1.y)) * (t - pt1.y) + pt1.x;
    }
    return cv::Point2f(b, t);
  } else {  //已知x,求y
    if (pt2.x - pt1.x == 0) {
      b = pt1.y;
    } else {
      b = ((pt2.y - pt1.y) / (pt2.x - pt1.x)) * (t - pt1.x) + pt1.y;
    }
    return cv::Point2f(t, b);
  }
}

//已知2点，求第三点的x或y
cv::Point2f CalcuateTools::GetPointByDoublePoint(cv::Point2f pt1,
                                                 cv::Point2f pt2, double t,
                                                 bool yFlag) {
  double b;
  if (yFlag) {  //已知y, 求x
    if (abs(pt2.y - pt1.y) < 1) {
      b = pt1.x;
    } else {
      b = ((pt2.x - pt1.x) / (pt2.y - pt1.y)) * (t - pt1.y) + pt1.x;
    }
    return cv::Point2f(b, t);
  } else {  //已知x,求y
    if (abs(pt2.x - pt1.x) < 1) {
      b = pt1.y;
    } else {
      b = ((pt2.y - pt1.y) / (pt2.x - pt1.x)) * (t - pt1.x) + pt1.y;
    }
    return cv::Point2f(t, b);
  }
}

float CalcuateTools::GetXorY(cv::Point pt1, cv::Point pt2, double t,
                             bool yFlag) {
  double b;
  if (yFlag)  //已知y,求x
  {
    if (pt2.y - pt1.y == 0)
      b = pt1.x;
    else
      b = ((pt2.x - pt1.x) / (pt2.y - pt1.y)) * (t - pt1.y) + pt1.x;
    return b;
  } else  //已知x,求y
  {
    if (pt2.x - pt1.x == 0)
      b = pt1.y;
    else
      b = ((pt2.y - pt1.y) / (pt2.x - pt1.x)) * (t - pt1.x) + pt1.y;
    return b;
  }
}

void CalcuateTools::CalcLinePara(cv::Point pt1, cv::Point pt2, double& fk,
                                 double& fb, double& frho, int& nDistance) {
  double x_diff = pt1.x - pt2.x;
  if (abs(x_diff) < 1.0) {
    return;
  }

  double y_diff = pt1.y - pt2.y;
  if (abs(y_diff) < 1e-6) {
    return;
  }

  fk = (1.0 * x_diff) / (1.0 * y_diff);

  fb = 1.0 * pt1.x - fk * pt1.y;
  frho = atan(1 / fk) * 180 / kPI;

  nDistance = (int)sqrt(1.0 * x_diff * x_diff + 1.0 * y_diff * y_diff);
}

float CalcuateTools::CalRho(cv::Point pt1, cv::Point pt2) {
  float frho = 0;
  if (abs(pt1.x - pt2.x) < 2)
    frho = 90;
  else {
    double fk = (1.0 * (pt1.y - pt2.y)) / (1.0 * (pt1.x - pt2.x));
    frho = atan(fk) * 180 / kPI;
  }
  // frho = atan2(1.0*( pt1.y - pt2.y ),1.0*( pt1.x - pt2.x ))*180/PI;
  return frho;
}

double CalcuateTools::PointToLineDistByPoint(cv::Point pt, cv::Point bpt1,
                                             cv::Point bpt2) {
  double tmp = abs(bpt1.x - bpt2.x);
  if (tmp < 1.0) {
    tmp = 1.0;
  }

  double a = (bpt2.y - bpt1.y) / tmp;
  double b = -1;
  double c = -a * bpt1.x + bpt1.y;
  double dis = a * pt.x + b * pt.y + c;
  dis = dis * dis;
  dis = dis / (a * a + b * b);

  return dis;
}

cv::Point2f CalcuateTools::GetCrossPoint(double a1, double b1, double a2,
                                         double b2) {
  double a_diff = a2 - a1;
  if (abs(a_diff) < 1e-6) {
    return cv::Point2f(0, 0);
  }

  double x = (b1 - b2) / a_diff;
  double y = a1 * x + b1;

  return cv::Point2f(y, x);
}

cv::Point2f CalcuateTools::GetCrossPoint(cv::Point point1, cv::Point point2,
                                         cv::Point point3, cv::Point point4) {
  cv::Point2f cross_point(0, 0);
  int pointA_x = point1.x - point2.x;
  int pointA_y = point1.y - point2.y;
  int pointB_x = point3.x - point4.x;
  int pointB_y = point3.y - point4.y;
  if (abs(pointA_x) < 1 && abs(pointB_x) < 1) {
    return cross_point;
  } else if (abs(pointA_x) < 1) {
    cross_point = GetPointByDoublePoint(point3, point4, point1.x, 0);
  } else if (abs(pointB_x) < 1) {
    cross_point = GetPointByDoublePoint(point1, point2, point3.x, 0);
  } else {
    double a1 = 1.0 * pointA_y / (1.0 * pointA_x);
    double a2 = 1.0 * pointB_y / (1.0 * pointB_x);

    double a_diff = a2 - a1;
    if (abs(a_diff) < 1e-6) {
      return cross_point;
    }

    double b1 = point1.y - a1 * (point1.x);
    double b2 = point3.y - a2 * (point3.x);

    cross_point.x = (b1 - b2) / a_diff;
    cross_point.y = a1 * cross_point.x + b1;
  }

  return cross_point;
}

double CalcuateTools::GetDoubleLineAngle(double k1, double k2) {
  double dUp = abs(k2 - k1);
  double dDown = abs(1 + k1 * k2);
  if (dDown <= 1e-6) {
    return 90;
  } else {
    double tan = dUp / dDown;
    return atan(tan) * 180 / kPI;
  }
}

short CalcuateTools::GetInt(short a, short b, short c) {
  a = a << 2;
  b = b << 1;
  return a | b | c;
}

void CalcuateTools::GetBinaryValue(short s, short& a, short& b, short& c) {
  a = (s >> 2) & 1;
  b = (s >> 1) & 1;
  c = s & 1;
}

void CalcuateTools::ShowPointsInMat(cv::Mat& input,
                                    std::vector<cv::Point> input_points,
                                    cv::Scalar color) {
  int len = input_points.size();
  for (int k = 0; k < len; ++k) {
    cv::circle(input, input_points[k], 1, color, 3);
  }
}
}
